# 间接平差 Adjustment of observation equation
import numpy as np

# 观测高差
L = np.mat([5.835, 3.782, 9.640, 7.384, 2.270]).transpose()

# 路线长度
S = [3.5, 2.7, 4.0, 3.0, 2.5]

# 已知高程
H_A = 237.483

# 权阵
P = np.mat(np.zeros([len(L), len(L)]))
for i in range(0, len(S)):
	P[i, i] = 1 / S[i]

# 观测方程系数阵
B = np.mat([
	[1, 0, 0],
	[-1, 1, 0],
	[0, 1, 0],
	[0, 1, -1],
	[0, 0, 1]
])

# 观测方程常数项向量
d = np.mat([-H_A, 0, -H_A, 0, -H_A]).transpose()
# 参数近似值向量
X0 = np.mat([H_A + L[0, 0], H_A + L[2, 0], H_A + L[4, 0]]).transpose()

# 误差方程常数项
l = L - (B*X0 + d)

# 法方程系数阵&常数项向量
Nbb = B.transpose()*P*B
W = B.transpose()*P*l

# 参数近似值改正数
xh = np.linalg.inv(Nbb) * W

# 改正数
V = B * xh - l
# 平差值
Lh = L + V
# 参数估值
Xh = X0 + xh

print("观测高差 L = \n", L)
print("观测方程系数阵 B = \n", B)
print("观测方程常数项 d = \n", d)
print("权阵 P = \n", P)
print("参数近似值 X0 = \n", X0)
print("误差方程常数项 l = \n", l)
print("法方程系数阵 Nbb = \n", Nbb)
print("法方程常数项向量 W = \n", W)
print("参数近似值改正数 xh = \n", xh)
print("改正数 V = \n", V)
print("平差值 Lh = \n", Lh)
print("参数估值 Xh = \n", Xh)
